#include "Graph.h"
#include <iostream>
#include <algorithm>

Graph::Graph(void)
{
}

Graph* Graph::getInstance(void)
{
	static Graph graph;
	return &graph;
}

Graph::~Graph(void)
{
}

void Graph::destroy(void)
{
	while(!mNodes.empty())
	{
		delete mNodes.back();
		mNodes.pop_back();
	}
}

PathNode* Graph::addNode(const std::string& name, Ogre::Vector3 pos,Ogre::SceneManager* pSceneMgr)
{
	PathNode* pNode = new PathNode(name, pos, pSceneMgr);
	mNodes.push_back(pNode);

	return pNode;
}

void Graph::connect(PathNode* pFirst, const std::string& second, Ogre::SceneManager* pSceneMgr)
{
	PathNode* pSecond = findNode(second);

	if(pFirst != NULL && pSecond != NULL)
	{
		pFirst->createEdge(pSecond);
		pSecond->createEdge(pFirst);
	}
}

PathNode* Graph::findNode(const std::string& name)
{
	PathNode* pNode = NULL;

	for(std::vector<PathNode*>::iterator itr = mNodes.begin(); itr != mNodes.end(); ++itr)
	{
		if((*itr)->getName().compare(name) == 0)
		{
			pNode = (*itr);
			break;
		}
	}

	assert(pNode != NULL);
	return pNode;
}

PathNode* Graph::findRandomNode(void)
{
	int index = (int)Ogre::Math::RangeRandom(0.0f,mNodes.size()-1);
	return mNodes.at(index);
}

bool Graph::findPath(const std::string& start, const std::string& end, std::stack<PathNode*>* pPath)
{
	if(start.compare(end) == 0)
	{
		return false;
	}

	PathNode* pStart = NULL;
	PathNode* pEnd = NULL;

	pStart = findNode(start);
	pEnd = findNode(end);

	if(pStart != NULL && pEnd != NULL)
		return findPath(pStart, pEnd, pPath);
	return false;
}

bool Graph::findPath(PathNode* pStart, PathNode* pEnd, std::stack<PathNode*>* pPath)
{
	if(pStart == NULL || pEnd == NULL)
		return false;

	if(pStart == pEnd)
	{
		return true;
	}

	std::vector<PathNode*> openList;

	openList.push_back(pStart);
	PathNode* pCurrNode = NULL;

	while(!openList.empty())
	{
		//Get best node from open list (lowest F value).
		//Since we sort the list at the end of the previous loop we know
		//the front node is the best
		pCurrNode = openList.front();

		//Exit if we're are the goal
		if(pCurrNode == pEnd)
			break;

		//Remove the node from the open list and place it in the closed
		openList.erase(openList.begin());
		pCurrNode->setClosed(true); //We use a flag instead of a list for speed

		//Test all of the edge nodes from the current node
		std::vector<Edge*>* pEdges = pCurrNode->getEdges();
		for(std::vector<Edge*>::iterator i = pEdges->begin(); i != pEdges->end(); ++i)
		{
			PathNode* pEdgeNode = (*i)->pNode;
			//If it's closed we've already analysed it
			if(!pEdgeNode->getClosed())
			{
				#ifdef _DEBUG
					pEdgeNode->setMaterial("Examples/RustySteel");
				#endif
				float cost = calculateCost(pCurrNode,(*i));
				
				if(!inList(pEdgeNode,&openList))
				{
					openList.push_back(pEdgeNode);
					pEdgeNode->setGCost(cost);
					pEdgeNode->calcHCost(pEnd);
					pEdgeNode->calcFCost();
					pEdgeNode->setParent(pCurrNode);
				}
				else
				{
					//If this is a better node (lower G cost)
					if(pEdgeNode->getGCost() > cost)
					{
						pEdgeNode->setGCost(cost);
						pEdgeNode->calcFCost();
						pEdgeNode->setParent(pCurrNode);
					}
				}
			}
		}

		//Place the lowest F cost item in the open list at the top, so we can
		//access it easily next iteration
		std::sort(openList.begin(), openList.end(),  Graph::compareNodes);
	}

	//Make sure we actually found a path
	float totalcost = 0;
	if(pEnd->getParent() != NULL)
	{
		while(pCurrNode != NULL)
		{
#ifdef _DEBUG
			pCurrNode->setMaterial("Examples/GrassFloor");
#endif
			pPath->push(pCurrNode);
			totalcost += pCurrNode->getFCost();
			pCurrNode = pCurrNode->getParent();
		}

		reset();
		return true;
	}

	return false;
}

float Graph::calculateCost(PathNode* pNode, Edge* pEdge)
{
	float cost = pNode->getGCost();

	cost += pEdge->moveCost;

	return cost;
}

bool Graph::inList(PathNode* pNode, std::vector<PathNode*>* pList)
{
	for(std::vector<PathNode*>::iterator i = pList->begin(); i != pList->end(); ++i)
	{
		if((*i) == pNode)
		{
			return true;
		}
	}

	return false;
}

bool Graph::compareNodes(PathNode* pFirst, PathNode* pSecond)
{
	return pFirst->getFCost() < pSecond->getFCost();
}

void Graph::reset(void)
{
	for(std::vector<PathNode*>::iterator i = mNodes.begin(); i != mNodes.end(); ++i)
	{
		(*i)->reset();
	}
}

/*void Graph::calculateExposures(std::vector<StaticObject*>* pObjects, Ogre::SceneManager* pSceneMgr)
{
	for(std::vector<PathNode*>::iterator pathItr = mNodes.begin(); pathItr != mNodes.end(); ++pathItr)
	{
		std::vector<Edge*>* pEdgeList = (*pathItr)->getEdges();
		for(std::vector<Edge*>::iterator edgeItr = pEdgeList->begin(); edgeItr != pEdgeList->end(); ++edgeItr)
		{
			Ogre::Vector3 pos = (*edgeItr)->pNode->getPosition().midPoint((*pathItr)->getPosition());

			(*edgeItr)->exposure = 10000;
			for(std::vector<StaticObject*>::iterator objItr = pObjects->begin(); objItr != pObjects->end(); ++objItr)
			{
				Ogre::Real d = pos.distance((*objItr)->getPosition());
				if(pos.distance((*objItr)->getPosition()) <= 200)
				{
					if((*edgeItr)->exposure == 10000)
					{
						(*edgeItr)->exposure = 20;
					}
					else if((*edgeItr)->exposure == 20)
					{
						(*edgeItr)->exposure = 1;
						break;
					}
				}
			}

#ifdef _DEBUG
			//Modified from http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Line+3D
			char name[24];
			sprintf(name,"%s-%s",(*pathItr)->getName().c_str(), (*edgeItr)->pNode->getName().c_str());
			Ogre::ManualObject* pLine =  pSceneMgr->createManualObject(); 
			Ogre::SceneNode* pLineNode = pSceneMgr->getRootSceneNode()->createChildSceneNode(); 
 
			Ogre::MaterialPtr lineMat = Ogre::MaterialManager::getSingleton().create(name,Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
			lineMat->setReceiveShadows(false); 
			lineMat->getTechnique(0)->setLightingEnabled(true);
			if((*edgeItr)->exposure == 1000)
			{
				lineMat->getTechnique(0)->getPass(0)->setDiffuse(1,0,0,0); 
				lineMat->getTechnique(0)->getPass(0)->setAmbient(1,0,0);
				lineMat->getTechnique(0)->getPass(0)->setSelfIllumination(1,0,0);
			}
			else if((*edgeItr)->exposure == 200)
			{
				lineMat->getTechnique(0)->getPass(0)->setDiffuse(0,0,1,0); 
				lineMat->getTechnique(0)->getPass(0)->setAmbient(0,0,1);
				lineMat->getTechnique(0)->getPass(0)->setSelfIllumination(0,0,1);
			}
			else if((*edgeItr)->exposure == 1)
			{
				lineMat->getTechnique(0)->getPass(0)->setDiffuse(0,1,0,0); 
				lineMat->getTechnique(0)->getPass(0)->setAmbient(0,1,0);
				lineMat->getTechnique(0)->getPass(0)->setSelfIllumination(0,1,0);
			}
 
			pLine->begin(name, Ogre::RenderOperation::OT_LINE_LIST); 
			pLine->position((*pathItr)->getPosition()); 
			pLine->position((*edgeItr)->pNode->getPosition()); 
			pLine->end(); 
 
			pLineNode->attachObject(pLine);
#endif
		}
	}
}*/